/*
 **********************************************************************
 * File name: ekf_att.cpp
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#include "ekf_att.h"
/*
*/
EKF_Att::EKF_Att() :
    q(1.0f, 0, 0, 0),
    accel_initialized(false),
    magn_initialized(false),
    _counter(0),
    _accel_sum(0),
    _magn_sum(0),
    _w_b{0,0,0}
  {
    _P11.set_identity();
    _P22.set_identity();
    _P12.set_zero();
    _P21.set_zero();
    
    _Q11.set_all((0.15f/57.3f)*(0.15f/57.3f)); //noise = 0.25 dps
    _Q22.set_all(1e-5f);
    _Ra.set_all(0.025f*0.025f);  //noise = 25mg
    _Rm.set_all(0.025f*0.025f);  //noise = 25mGauss
  }
/*
*/
bool EKF_Att::initialize_with_accel(Vector3f &accel)
{
  _accel_sum -= accel;  //must change accel to negative
  
  if(++_counter == 100) {
    _accel_sum /= 100;
    _accel_sum.normalize();
    Vector3f vg(0, 0, 1.0f);
    Vector3f axis = vg.cross(_accel_sum);
    float angle = asinf(axis.norm()) / 2.0f;
    float cos_angle = cosf(angle);
    float sin_angle = sinf(angle);
    axis = axis.normalized() * sin_angle;
    q = Quaternion(cos_angle, axis);
    _accel_sum.set_zero();
    _counter = 0;
    accel_initialized = true;
    return true;
  }
  
  return false;
}
/*
*/
bool EKF_Att::initialize_with_magn(Vector3f &magn)
{
  return false;
}
/*
*/
void EKF_Att::predict_with_gyro(Vector3f &gyro, float dt)
{
  /*----------------------------*
   *   we = w - wb              *
   *----------------------------*/
  _w_e = gyro - _w_b;
  
  /*----------------------------*
   *   q = dq*q                 *
   *----------------------------*/
  Quaternion dq(1, _w_e*(dt*0.5f));
  q = dq*q;
  q.normalize();
  
  /*------------------------------*
   *   F=[F11 F12]                *
   *     [ 0   I ]                *
   * where F11 = I - skew(w)*dt    *
   *       F12 = -I*dt            *
   *------------------------------*/
  DMatrix3f I(1);
  SMatrix3f F11 = I - _w_e.skew_symmetric(dt);
  DMatrix3f F12(-dt);
  
  /*------------------------------*
   *   Pe = F*P*F' + Q            *
   *   M = F11*P11 + F12*P21      *
   *   N = F11*P12 + F12*P22      *
   *   P11 = M*F11' + N*F12'      *
   *   P12 = N                    *
   *   P21 = N'                   *
   *   P22 = P22                  *
   *------------------------------*/
  _P12 = F11*_P12 + F12*_P22;
  _P11 = (F11*_P11 + F12*_P21)*F11.T() + _P12*F12.T();
  _P21 = _P12.T(); 
  
  _P11 += _Q11*dt;
  _P22 += _Q22*dt;
}

/*
*/
void EKF_Att::correct_with_accel(Vector3f &accel)
{
  /*------------------------------*
   *   H = [H11 H12]              *
   *   H11 = skew(dcm(q)*[0 0 1]) *
   *   H12 = 0                    *
   *------------------------------*/
  DCM dcm(q);
  Vector3f ve(dcm.data[0][2], dcm.data[1][2], dcm.data[2][2]);
  SMatrix3f H11 = ve.skew_symmetric();
  
  /*------------------------------*
   *   S = H*P*H' + R             *
   *   K = P*H'*S^-1              *
   *------------------------------*/
  SMatrix3f K11, K21;
  K11 = _P11*H11.T();
  K21 = _P21*H11.T();
  SMatrix3f S = H11*K11 + _Ra;
  SMatrix3f Sinv;
  if(S.I(Sinv))
  {
    K11 *= Sinv;
    K21 *= Sinv;
    Vector3f vm = -accel.normalized();  //must change accel to negative
    Vector3f error = vm - ve;
    Vector3f dv = K11*error/2.0f;
    Vector3f db = K21*error;
    
    //correct q
    float norm = dv.dot(dv);
    if(norm > 1.0f)
    {
      norm = sqrtf(1.0f + norm);
      dv /= norm;
      Quaternion dq(1.0f/norm, dv.x, dv.y, dv.z);
      q = dq * q;
      q.normalize();
    }
    else
    {
      Quaternion dq(sqrtf(1.0f - norm), dv.x, dv.y, dv.z);
      q = dq * q;
      q.normalize();
    }
    
    //correct _w_b
    _w_b.x += db.x;
    _w_b.y += db.y;
    
    //correct P
    //P = (I - KH)*P
    DMatrix3f I(1.0f);
    SMatrix3f M11 = I - K11*H11;
    SMatrix3f M21 = -K21*H11;
    
    _P22 = M21*_P12 + _P22;
    _P11 = M11*_P11;
    _P12 = M11*_P12;
    _P21 = _P12.T();
  }
}

/*
*/
void EKF_Att::set_gyro_noise(Vector3f noise)
{
  _Q11.v[0] = noise.v[0] * noise.v[0];
  _Q11.v[1] = noise.v[1] * noise.v[1];
  _Q11.v[2] = noise.v[2] * noise.v[2];
}
/*
*/
void EKF_Att::set_accel_noise(Vector3f noise)
{
  _Ra.v[0] = noise.v[0] * noise.v[0];
  _Ra.v[1] = noise.v[1] * noise.v[1];
  _Ra.v[2] = noise.v[2] * noise.v[2];
}
/*
*/
void EKF_Att::set_magn_noise(Vector3f noise)
{
  _Rm.v[0] = noise.v[0] * noise.v[0];
  _Rm.v[1] = noise.v[1] * noise.v[1];
  _Rm.v[2] = noise.v[2] * noise.v[2];
}





